#! /usr/bin/env python
# -*- coding: utf-8 -*

import os
import cv2 as cv
import numpy as np
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

haarshare_dir = "/home/oryxbot/oryxbot_ws/src/human_tracking/haarshare/"
cascade_file = "haarcascade_lowerbody.xml"
# cascade_file = "haarcascade_fullbody.xml"
rospy.init_node("human_tracking")
rate = rospy.Rate(50)
print(os.path.join(haarshare_dir,cascade_file))
cascade = cv.CascadeClassifier(os.path.join(haarshare_dir,cascade_file))

def image_callback(image):
    bridgr = CvBridge()
    image = bridgr.imgmsg_to_cv2(image,"bgr8")
    gray = cv.cvtColor(image,cv.COLOR_BGR2GRAY)
    objs = cascade.detectMultiScale(gray,scaleFactor=1.1,minNeighbors=2,minSize=(50,50))
    for obj in objs:
        cv.rectangle(image,(obj[0],obj[1]),(obj[0]+obj[2],obj[1]+obj[3]),(250,250,0),3)
        cv.circle(image,(obj[0],obj[1]),3,(255,0,0),-1)

    img = bridgr.cv2_to_imgmsg(image,"bgr8")
    rate.sleep()
    pub_img.publish(img)

sub_img = rospy.Subscriber("/berxel_camera/rgb/rgb_raw", Image, image_callback)
pub_img = rospy.Publisher("/human_image", Image, queue_size=10)
rospy.spin()
